/**
 * @file chassis_task.c
 * @brief 底盘任务执行文件
 *        包含底盘输入处理、模式控制、PID计算、功率限制及电机通信等核心逻辑
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 * @bug 待解决问题:
 */
///< 包含底盘核心定义头文件（结构体、枚举、宏等）
#include "chassis.h"
///< 包含功率控制模块头文件（用于底盘功率限制）
#include "power_ctrl.h"
///< 包含控制输入模块头文件（遥控器/键盘输入处理）
#include "ctrl.h"
///< 包含PID控制算法头文件（用于电机闭环控制）
#include "pid.h"
#include "power_ctrl.h"
///< 包含自定义数学库头文件（斜坡函数、三角函数等）
#include "QWF_Math.h"
///< 包含裁判系统通信头文件（比赛状态获取）
#include "judge.h"
///< 包含FreeRTOS内核头文件（任务管理）
#include "freertos.h"		//freertos
///< 包含FreeRTOS任务头文件（任务延时函数）
#include "task.h"			//freertos暂停时长
///< 包含自定义串口通信头文件（VOFA数据发送）
#include "QWF_Usart.h"
/************************宏定义******************/
///< 圆周率宏定义（用于角度计算）
#ifndef PI
  #define PI               3.14159265358979f
#endif
/**
 * @brief 半舵轮底盘输入转换函数
 *        将遥控器/键盘输入转换为底盘X/Y轴目标速度
 *        支持RC（遥控器）和KM（键鼠）两种控制模式
 */
void rc_chance(void){
	/*通过遥控器控制机体整体期望状态
		90
		+|+
	0---	---180/-180
		-|-
		-90
	*/
	if(ctrl->mode==RC){//遥控器
		        // 遥控器右摇杆上下通道（CH2）转换为X轴目标速度（反向处理）
        chassis->sys.v.x.target[now]=-ctrl->RC[now].ch[2]/660.0f*8000;
		        // 遥控器右摇杆左右通道（CH3）转换为Y轴目标速度（反向处理）
        chassis->sys.v.y.target[now]=-ctrl->RC[now].ch[3]/660.0f*8000;
	}
	else if(ctrl->mode==KM){
		
//		chassis->sys.v.x.target[now]=QWF_Ramp((-(rc_key(time).d-rc_key(time).a)*chassis_normal_v_max),chassis->sys.v.x.target[now],15);
//		chassis->sys.v.y.target[now]=QWF_Ramp((-(rc_key(time).w-rc_key(time).s)*chassis_normal_v_max),chassis->sys.v.y.target[now],15);		
		chassis->sys.v.y.target[now]=(-(ctrl->KM[now].key.w-ctrl->KM[now].key.s)*8000);		
		chassis->sys.v.x.target[now]=(-(ctrl->KM[now].key.d-ctrl->KM[now].key.a)*8000);
		
	}
	        // 计算底盘合速度（X/Y轴速度的矢量和）
        chassis->sys.v.all.target[now]=sqrt(chassis->sys.v.x.target[now]*chassis->sys.v.x.target[now]+chassis->sys.v.y.target[now]*chassis->sys.v.y.target[now]);
//	if(chassis->sys.v.y.target[now]*chassis->sys.v.y.target[last]!=-1){
//	chassis->sys.v.y.target[now]=QWF_Ramp(chassis->sys.v.y.target[now],chassis->sys.v.y.target[last],15);
//	}
//	chassis->sys.v.y.target[last]=chassis->sys.v.y.target[now];	
//	sys_v=sqrt(chassis->sys.v.x.target[now]*chassis->sys.v.x.target[now]+chassis->sys.v.y.target[now]*chassis->sys.v.y.target[now]);
//	chassis_w_max=2200;
//	chassis_w_max=8000;	
//		sys_angle=(sys_v)?(chassis->sys.v.y.target[now]?	atan((float)chassis->sys.v.x.target[now]/(float)chassis->sys.v.y.target[now])/PI*180:	(chassis->sys.v.x.target[now]>0?-90:90)):sys_angle;
}
/**
 * @brief 底盘主任务函数
 *        周期执行底盘控制逻辑（1ms周期）
 *        包含输入处理、模式控制、PID计算、功率限制、电机数据发送
 * @param argument FreeRTOS任务参数（未使用）
 * @return 无（任务函数不返回）
 */
void chassis_task(void const * argument)
{
	static TickType_t xLastWakeTime;  // FreeRTOS任务唤醒时间戳（用于周期控制）
    const TickType_t xPeriod = pdMS_TO_TICKS(1); // 任务执行周期（1ms）
    // 初始化唤醒时间
    xLastWakeTime = xTaskGetTickCount();  // 初始化唤醒时间戳
	while(1){
		rc_chance();  // 执行输入转换（遥控器/键盘→目标速度）
		chassis->ctrl_func[chassis->mode[now]!=chassis_stop]();  // 执行非停止模式控制函数
		if(ctrl->type!=Null_Ctrl)
			chassis->ctrl_func[chassis->mode[now]]();
		else
			chassis->ctrl_func[chassis->mode[now]=chassis_stop]();
		        // 舵轮角速度PID计算（目标值→输出量）
        PID_Calculate(&chassis_pid->f.helm.w,	wheel_f_w_meas,	 wheel_f_w_target);
		PID_Calculate(&chassis_pid->b.helm.w,	wheel_b_w_meas,	 wheel_b_w_target);
				//所有轮子pid计算
		PID_Calculate(&chassis_pid->f.wheel.v,	wheel_f_v_meas,  wheel_f_v_target);
		PID_Calculate(&chassis_pid->b.wheel.v,	wheel_b_v_meas,  wheel_b_v_target);
		PID_Calculate(&chassis_pid->l.wheel.v,	wheel_l_v_meas,  wheel_l_v_target);
		PID_Calculate(&chassis_pid->r.wheel.v,	wheel_r_v_meas,  wheel_r_v_target);

		Chassis_Power_Control();//功率控制	
		        // 前舵轮电机输出赋值（PID计算结果转换为16位整型）
        chassis->part.f.helm.target.out =(int16_t)chassis_pid->f.helm.w.output ;
		chassis->part.f.wheel.target.out=(int16_t)chassis_pid->f.wheel.v.output ;
		chassis->part.b.helm.target.out =(int16_t)chassis_pid->b.helm.w.output ;
		chassis->part.b.wheel.target.out=(int16_t)chassis_pid->b.wheel.v.output ;
		chassis->part.l.wheel.target.out=(int16_t)chassis_pid->l.wheel.v.output ;
		chassis->part.r.wheel.target.out=(int16_t)chassis_pid->r.wheel.v.output ;
		/*-----------------------发送数据给电机---------------------------------------------*/
		send_data_chassis_gm6020(f.helm); 
		send_data_chassis_gm6020(b.helm);
		send_data_chassis_c620	(f.wheel);
		send_data_chassis_c620	(b.wheel);	
		send_data_chassis_c620	(l.wheel);
		send_data_chassis_c620	(r.wheel);
		/*-----------------------发送数据给VOFA---------------------------------------------*/
		Add_Data_To_Vofa(chassis_pid->f.helm.angle.target[now],f_angle_target);  // 前舵轮目标角度发送至VOFA调试工具
		Add_Data_To_Vofa(chassis_pid->f.helm.angle.measure[now],f_angle_meas);
		Add_Data_To_Vofa(chassis_pid->b.helm.angle.target[now],b_angle_target)
		Add_Data_To_Vofa(chassis_pid->b.helm.angle.measure[now],b_angle_meas);
		Add_Data_To_Vofa(chassis_pid->f.wheel.v.target[now],f_v_target)
		Add_Data_To_Vofa(chassis_pid->f.wheel.v.measure[now],f_v_meas);
		Add_Data_To_Vofa(chassis_pid->b.wheel.v.target[now],b_v_target)
		Add_Data_To_Vofa(chassis_pid->b.wheel.v.measure[now],b_v_meas);	
		Add_Data_To_Vofa(chassis_pid->l.wheel.v.target[now],l_v_target)
		Add_Data_To_Vofa(chassis_pid->l.wheel.v.measure[now],l_v_meas);
		Add_Data_To_Vofa(chassis_pid->r.wheel.v.target[now],r_v_target)
		Add_Data_To_Vofa(chassis_pid->r.wheel.v.measure[now],r_v_meas);			
		vTaskDelayUntil(&xLastWakeTime, xPeriod);  // 任务周期延时（确保1ms执行一次）	
	}
}
